#include "icp_localizer.h"

ICPLocalizer::ICPLocalizer(const ICPConfig &config) : m_config(config)
{
    m_refine_inp.reset(new CloudType);
    m_refine_tgt.reset(new CloudType);
    m_rough_inp.reset(new CloudType);
    m_rough_tgt.reset(new CloudType);
}
bool ICPLocalizer::loadMap(const std::string &path)
{
    if (!std::filesystem::exists(path))
    {
        std::cerr << "Map file not found: " << path << std::endl;
        return false;
    }
    
    try
    {
        pcl::PCDReader reader;
        CloudType::Ptr cloud(new CloudType);
        
        if (reader.read(path, *cloud) == -1)
        {
            std::cerr << "Failed to read PCD file: " << path << std::endl;
            return false;
        }
        
        std::cout << "Load map point size: " << cloud->size() << std::endl;
        
        if (cloud->empty())
        {
            std::cerr << "Loaded point cloud is empty!" << std::endl;
            return false;
        }
        
        // 精配准地图 - 使用独立的滤波器对象避免状态冲突
        if (m_config.refine_map_resolution > 0)
        {
            std::cout << "Filtering refine map with resolution: " 
                      << m_config.refine_map_resolution << std::endl;
            
            // 创建独立的滤波器实例，避免重用导致的内部状态问题
            pcl::VoxelGrid<PointType> refine_filter;
            refine_filter.setLeafSize(m_config.refine_map_resolution, 
                                     m_config.refine_map_resolution, 
                                     m_config.refine_map_resolution);
            refine_filter.setInputCloud(cloud);
            
            // 清空目标点云，释放旧数据
            m_refine_tgt->clear();
            
            // 体素滤波可能失败，需要捕获异常
            refine_filter.filter(*m_refine_tgt);
            
            std::cout << "Refine map filtered to: " << m_refine_tgt->size() 
                      << " points" << std::endl;
        }
        else
        {
            m_refine_tgt->clear();
            pcl::copyPointCloud(*cloud, *m_refine_tgt);
            std::cout << "Refine map (no filter): " << m_refine_tgt->size() 
                      << " points" << std::endl;
        }
        
        // 粗配准地图 - 使用独立的滤波器对象
        if (m_config.rough_map_resolution > 0)
        {
            std::cout << "Filtering rough map with resolution: " 
                      << m_config.rough_map_resolution << std::endl;
            
            // 创建独立的滤波器实例
            pcl::VoxelGrid<PointType> rough_filter;
            rough_filter.setLeafSize(m_config.rough_map_resolution, 
                                    m_config.rough_map_resolution, 
                                    m_config.rough_map_resolution);
            rough_filter.setInputCloud(cloud);
            
            // 清空目标点云，释放旧数据
            m_rough_tgt->clear();
            
            // 体素滤波可能失败，需要捕获异常
            rough_filter.filter(*m_rough_tgt);
            
            std::cout << "Rough map filtered to: " << m_rough_tgt->size() 
                      << " points" << std::endl;
        }
        else
        {
            m_rough_tgt->clear();
            pcl::copyPointCloud(*cloud, *m_rough_tgt);
            std::cout << "Rough map (no filter): " << m_rough_tgt->size() 
                      << " points" << std::endl;
        }
        
        // 释放原始点云内存
        cloud->clear();
        
        std::cout << "Map loaded successfully!" << std::endl;
        return true;
    }
    catch (const std::bad_alloc &e)
    {
        std::cerr << "Memory allocation failed during map loading: " << e.what() << std::endl;
        std::cerr << "Try increasing voxel resolution (rough_map_resolution, refine_map_resolution)" << std::endl;
        
        // 清理已分配的内存
        m_refine_tgt->clear();
        m_rough_tgt->clear();
        return false;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception during voxel filtering: " << e.what() << std::endl;
        
        // 清理已分配的内存
        m_refine_tgt->clear();
        m_rough_tgt->clear();
        return false;
    }
}
void ICPLocalizer::setInput(const CloudType::Ptr &cloud)
{
    try
    {
        if (m_config.refine_scan_resolution > 0)
        {
            // 使用独立的滤波器实例
            pcl::VoxelGrid<PointType> refine_filter;
            refine_filter.setLeafSize(m_config.refine_scan_resolution, 
                                     m_config.refine_scan_resolution, 
                                     m_config.refine_scan_resolution);
            refine_filter.setInputCloud(cloud);
            
            m_refine_inp->clear();
            refine_filter.filter(*m_refine_inp);
        }
        else
        {
            m_refine_inp->clear();
            pcl::copyPointCloud(*cloud, *m_refine_inp);
        }

        if (m_config.rough_scan_resolution > 0)
        {
            // 使用独立的滤波器实例
            pcl::VoxelGrid<PointType> rough_filter;
            rough_filter.setLeafSize(m_config.rough_scan_resolution, 
                                    m_config.rough_scan_resolution, 
                                    m_config.rough_scan_resolution);
            rough_filter.setInputCloud(cloud);
            
            m_rough_inp->clear();
            rough_filter.filter(*m_rough_inp);
        }
        else
        {
            m_rough_inp->clear();
            pcl::copyPointCloud(*cloud, *m_rough_inp);
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << "Exception during scan voxel filtering: " << e.what() << std::endl;
        // 清理数据
        m_refine_inp->clear();
        m_rough_inp->clear();
    }
}

bool ICPLocalizer::align(M4F &guess)
{
    CloudType::Ptr aligned_cloud(new CloudType);
    if (m_refine_tgt->size() == 0 || m_rough_tgt->size() == 0)
        return false;
    m_rough_icp.setMaximumIterations(m_config.rough_max_iteration);
    m_rough_icp.setInputSource(m_rough_inp);
    m_rough_icp.setInputTarget(m_rough_tgt);
    m_rough_icp.align(*aligned_cloud, guess);
    if (!m_rough_icp.hasConverged() || m_rough_icp.getFitnessScore() > m_config.rough_score_thresh)
        return false;
    m_refine_icp.setMaximumIterations(m_config.refine_max_iteration);
    m_refine_icp.setInputSource(m_refine_inp);
    m_refine_icp.setInputTarget(m_refine_tgt);
    m_refine_icp.align(*aligned_cloud, m_rough_icp.getFinalTransformation());
    if (!m_refine_icp.hasConverged() || m_refine_icp.getFitnessScore() > m_config.refine_score_thresh)
        return false;
    guess = m_refine_icp.getFinalTransformation();
    return true;
}